// -*- coding: utf-8 -*-

#include <iostream>
#include <string>

#include "elisa/elisa.h"

#include "Modeler.h"

//#define SLOW_CURVE

using namespace std;
using namespace elisa;

class Falling1 : public Modeler
{
private:
	static const VNInterval G, K, E;

	ContVar px, py;
	ContVar vx, vy;

	static Interval Time;
	static VNInterval PX, PY;
	static VNInterval VX, VY;

public:
	Falling1()
		: Modeler(Time),
		  px(PX, "px"),
		  py(PY, "py"),
		  vx(VX, "vx"),
		  vy(VY, "vy")
	{
		double p((Time.width() + PX.width() + VX.width() + PY.width() + VY.width()) / 5);
		if (p != 0.0)
			elisa::RealVar::SetPrecision(p);
		else
			elisa::RealVar::SetPrecision(1.0e-1);
		// FIXME
		elisa::RealVar::SetPrecision(1.0e-1);

		t.setPrecision(1.0e-2);
/*
		p = 1.0e-2;
		//p = 1.0e-11;
		t.setPrecision(std::max<double>(p, Time.width()*10));

		elisa::RealVar::SetPrecision(std::max<double>(p, Time.width()*10));
*/

		//t.setDomain(new Interval(Time.inf(), 1.0e2));
		//t.setDomain(new Interval(Time.inf(), 1.0));
		t.setDomain(new Interval(Time.sup() + 1.0e-4, 1.0e2));

		addVar(px);
		addVar(py);
		addVar(vx);
		addVar(vy);
	}

	// 連続制約の定義
	template<typename T>
	static void flow(int n, T *dv, const T *v, T t, void *param)
	{
		// x
		dv[0] = v[2];
		// y
		dv[1] = v[3];
		// vx
		dv[2] = T(0.0);
		// vy
		//dv[3] = -G - K*v[3];
		dv[3] = -G + K*sqr(v[3]);
	}

	// ガード制約の定義
	template<typename T> 
	static T guard(const std::vector<T>& v, const T& t)
	{
#ifdef SLOW_CURVE
		return v[1] - sin(v[0]);
#else
		return v[1] - sin(T(2.0)*v[0]);
#endif
	}

	bool reset(bool first)
	{
		//return first ? true : false;

		Interval nt(t.domain().inf(), t.domain().sup());
		VNInterval nx(px.domain().inf(), px.domain().sup());
		VNInterval ny(py.domain().inf(), py.domain().sup());
		VNInterval pvx(vx.domain().inf(), vx.domain().sup());
		VNInterval pvy(vy.domain().inf(), vy.domain().sup());

#ifdef SLOW_CURVE
		// y=sin(x) の場合の跳ね返り
		VNInterval nvx( ((VNInterval(1) - E*v_bias::pow(v_bias::cos(nx), 2)) * pvx + (VNInterval(1)+E) * v_bias::cos(nx) * pvy) 
			/ (VNInterval(1) + v_bias::pow(v_bias::cos(nx), 2)) );
		VNInterval nvy( ((VNInterval(1)+E) * v_bias::cos(nx) * pvx + (-E + v_bias::pow(v_bias::cos(nx), 2)) * pvy) 
			/ (VNInterval(1) + v_bias::pow(v_bias::cos(nx), 2)) );
#else
		// y=sin(2x) の場合の跳ね返り
/*		VNInterval nvx( ((VNInterval(1) - E*v_bias::pow(2*v_bias::cos(2*nx), 2)) * pvx + (VNInterval(1)+E) * 2*v_bias::cos(2*nx) * pvy) 
			/ (VNInterval(1) + v_bias::pow(2*v_bias::cos(2*nx), 2)) );
		VNInterval nvy( ((VNInterval(1)+E) * 2*v_bias::cos(2*nx) * pvx + (-E + v_bias::pow(2*v_bias::cos(2*nx), 2)) * pvy) 
		/ (VNInterval(1) + v_bias::pow(2*v_bias::cos(2*nx), 2)) );*/
		VNInterval nvx( ((VNInterval(1) - E*pow(2*cos(2*nx), 2)) * pvx + (VNInterval(1)+E) * 2*cos(2*nx) * pvy) 
			/ (VNInterval(1) + pow(2*cos(2*nx), 2)) );
		VNInterval nvy( ((VNInterval(1)+E) * 2*cos(2*nx) * pvx + (-E + pow(2*cos(2*nx), 2)) * pvy) 
			/ (VNInterval(1) + pow(2*cos(2*nx), 2)) );
#endif

		/*
		// リセットしない
		VNInterval nvx(pvx);
		VNInterval nvy(pvy);
		*/

		if (first) { // 最初に見つかった解
			if (Time.overlaps(nt))
				// 前フェーズの終了時間と重なっていた場合は更新しない
				return false;

			Time = nt;
			PX = nx;
			PY = ny;
			VX = nvx;
			VY = nvy;
		
		} else { // 2つ目以降

/*
			if (!nextT.overlaps(nt))
				// これまでの解と重なりがない → 定性的に異なる解 (TODO)
				return false;
*/

			Time  = hull(Time, nt);
			PX = elisa::vn::hull(PX, nx);
			PY = elisa::vn::hull(PY, ny);
			VX = elisa::vn::hull(VX, nvx);
			VY = elisa::vn::hull(VY, nvy);
		}

		// 和をとった包囲を表示
		cout << '\t' << "nt " << " = " << Time  << endl;
		cout << '\t' << "npx" << " = " << PX << endl;
		cout << '\t' << "nvx" << " = " << VX << endl;
		cout << '\t' << "npy" << " = " << PY << endl;
		cout << '\t' << "nvy" << " = " << VY << endl;

		//return true;
		return first ? true : false;
	}
	
	void print(std::ostream& out) const
	{
		out << '\t' << t.getName() << " = " << t.domain() << endl;
		out << '\t' << "px" << " = " << px.domain() << endl;
		out << '\t' << "py" << " = " << py.domain() << endl;
		out << '\t' << "vx" << " = " << vx.domain() << endl;
		out << '\t' << "vy" << " = " << vy.domain() << endl;
	}
};

MODEL(Falling1)


const VNInterval Falling1::G(VNInterval(98)/10);
//const VNInterval Falling1::K(VNInterval(3)/10);
const VNInterval Falling1::K(VNInterval(1)/1000);
const VNInterval Falling1::E(0.7);

Interval Falling1::Time(Interval::Zero);
VNInterval Falling1::PX(0.0);
VNInterval Falling1::PY(VNInterval(1.0, 1.1));
//VNInterval Falling1::PY(VNInterval(1.1));
//VNInterval Falling1::PY(VNInterval(13)/10);
VNInterval Falling1::VX(VNInterval(41)/10);
VNInterval Falling1::VY(-VNInterval(4)/10);
//VNInterval Falling1::VY(-VNInterval(3)/10);

/*
// かすった後の跳ね返り（一部）
VNInterval Falling1::PX(0.916119585);
VNInterval Falling1::PY(0.966018044);
VNInterval Falling1::VX(VNInterval(41)/10);
VNInterval Falling1::VY(-2.58916081);
*/


/*
Interval Falling1::Time(Interval::zero);
VNInterval Falling1::PX(12.0);
VNInterval Falling1::PY(2.0);
VNInterval Falling1::VX(15.6);
VNInterval Falling1::VY(-6.0);

// TOM1の例
Interval Falling1::Time(Interval::Zero);
VNInterval Falling1::PX(2);
VNInterval Falling1::PY(5);
VNInterval Falling1::VX(0.0);
VNInterval Falling1::VY(-5);
*/

/*
// 2回目 (INewton)
Interval Falling1::Time(0.56636310070488161, 0.56636310071488161);
VNInterval Falling1::PX(1.9999999999999998, 2.0000000000000004);
VNInterval Falling1::PY(0.90929742673245084, 0.90929742682568659);
VNInterval Falling1::VX(5.9527505352761727, 5.9527505353209147);
VNInterval Falling1::VY(4.9812997879549306, 4.9812997879923699);

// 2回目 (w/o INewton)
Interval Falling1::Time(0.5663631006621711, 0.56636310074750651);
VNInterval Falling1::PX(1.9999999999999998, 2.0000000000000004);
VNInterval Falling1::PY(0.90929742642828126, 0.90929742722388551);
VNInterval Falling1::VX(5.9527505350851797, 5.9527505354668024);
VNInterval Falling1::VY(4.9812997877951064, 4.9812997881144483);

// 3回目 (INewton)
Interval Falling1::Time(1.5193134214000554, 1.5193134214242554);
VNInterval Falling1::PX(7.6726755316097863, 7.672675531796501);
VNInterval Falling1::PY(0.98360902263607652, 0.98360902286634488);
VNInterval Falling1::VX(4.238538248833799, 4.2385382509290777);
VNInterval Falling1::VY(5.1268884475138146, 5.126888450217157);

// 3回目 (w/o INewton)
Interval Falling1::Time(1.5193134212298953, 1.5193134215072406);
VNInterval Falling1::PX(7.6726755306690997, 7.6726755326837619);
VNInterval Falling1::PY(0.98360902164890407, 0.98360902392394622);
VNInterval Falling1::VX(4.2385382383068562, 4.2385382608427848);
VNInterval Falling1::VY(5.1268884344526766, 5.1268884636449785);

// ADHS'09の例
Interval   Falling1::Time(Interval::Zero);
VNInterval Falling1::PX(1.0);
VNInterval Falling1::PY(3.0);
VNInterval Falling1::VX(6.0);
VNInterval Falling1::VY(-2.0);

// 2回目
Interval Falling1::Time(0.482177734375, 0.48828125);
VNInterval Falling1::PX(3.9092385233227125, 3.9092385455811143);
VNInterval Falling1::PY(0.99936976839319303, 0.99936979121386937);
VNInterval Falling1::VX(5.3030621198251913, 5.3030630442340776);
VNInterval Falling1::VY(3.6652525243466623, 3.6652534590623564);

// 3回目
Interval Falling1::Time(1.4907205361845917, 1.4907207028412521);
VNInterval Falling1::PX(9.257603651821519, 9.2576054901766351);
VNInterval Falling1::PY(-0.32815309476060367, -0.328151288517466);
VNInterval Falling1::VX(-4.5203659232708295, -4.5203455821604823);
VNInterval Falling1::VY(-0.62058026702388913, -0.62056127513815884);
*/

/*
// きわどい場合
Interval Falling1::Time(Interval::Zero);
VNInterval Falling1::PX(1.0);
VNInterval Falling1::PY(3.0);
VNInterval Falling1::VX(6.0);
VNInterval Falling1::VY(-1.7, -1.6);
*/

// after 1 bounce
// split:  1
// hnr:    3
// narrow: 6
// time: 125
//
// time = [0.56636310070488172, 0.56636310070503759]
// x  = [1.9999999999999998, 2.0000000000000004]
// y  = [0.90929742682568138, 0.90929742682568193]
// vx = [-2.739161746200948e-021, 2.739161746200948e-021]
// vy = [-9.3231476149566479, -9.3231476076638806]
// nvx = [5.9527505306205128, 5.952750535276901]
// nvy = [4.9812997840590443, 4.9812997879555381]
//
/*
Interval Falling1::Time(0.56636310070488172, 0.56636310070503759);
VNInterval Falling1::PX(1.9999999999999998, 2.0000000000000004);
VNInterval Falling1::PY(0.90929742682568138, 0.90929742682568193);
VNInterval Falling1::VX(5.9527505306205128, 5.952750535276901);
VNInterval Falling1::VY(4.9812997840590443, 4.9812997879555381);
*/


// 分岐する例 (SIMUTools 2009)
/*
// 初期条件
Interval Falling1::Time(Interval::Zero);
VNInterval Falling1::PX(1.025, 1.05);
VNInterval Falling1::PY(5.0);
VNInterval Falling1::VX(0.0);
VNInterval Falling1::VY(-5.0);
*/

/*
// A1回目 (y=sin(x))
Interval   Falling1::Time (0.57084697625982739, 0.57220488450987683);
VNInterval Falling1::X (1.0249999999999959,  1.050000000000004);
VNInterval Falling1::Y (0.85471418903933927, 0.86850805029710931);
VNInterval Falling1::VX(-7.0132241371916457, -6.5992035953300174);
VNInterval Falling1::VY(3.9091205330313441,  4.1463590748842556);
*/

/*
// B1回目 (y=sin(2x))
Interval   Falling1::Time (0.56871377828165648, 0.57129736249139773);
VNInterval Falling1::X (1.0249999999999928, 1.0500000000000071);
VNInterval Falling1::Y (0.86320936551669292, 0.88850062100632621);
VNInterval Falling1::VX(7.6757617203443003, 9.1912646088842234);
VNInterval Falling1::VY(-1.109955225110868, -0.23284516867260219);
*/

/*
// A2回目 (y=sin(2x))
Interval   Falling1::Time (0.6023098890280858, 0.6101750807751799);
VNInterval Falling1::X (0.74918318814291385, 0.84236983294017431);
VNInterval Falling1::Y (0.97229251735154909, 1.0231536424068779);
VNInterval Falling1::VX(-8.5688112827240914, -5.0737536990108163);
VNInterval Falling1::VY(-4.8675006701234311, 0.26665623781285364);
*/

/*
// B2回目 (y=sin(x))
nt  = [0.57388094670113898, 0.57706650362476763]
nx  = [1.0646619535568203, 1.1267721088338838]
ny  = [0.85360790764851957, 0.88716764224493894]
nvx = [4.2060899864650159, 6.4290773542302366]
nvy = [4.9352023524116841, 7.3891084103162994]
*/

/*
// after 2 bounces
// split:  1
// hnr:    3
// narrow: 29
// time: 250
//
// time = [1.51931342141 86268, 1.51931342141 90114]
// x  = [7.672675531721 0387, 7.6726755317212447]
// y  = [0.9836090227544 3696, 0.98360902275447404]
// vx = [5.95275053527690 01, 5.9527505352769019]
// vy = [-4.3799068338 2107, -4.3799068338177953]
// nvx = [4.2385382500 281841, 4.2385382500314055]
// nvy = [5.12688844878 32915, 5.1268884487885078]
interval Falling1::Time(1.5193134214186268, 1.5193134214190114);
interval Falling1::PX(7.6726755317210387, 7.6726755317212447);
//interval Falling1::VX(4.2385382500281841, 4.2385382500314055);
interval Falling1::VX(4.2385382500281841, 4.23853825003);
interval Falling1::PY(0.98360902275443696, 0.98360902275447404);
//interval Falling1::VY(5.1268884487832915, 5.1268884487885078);
interval Falling1::VY(5.1268884487832915, 5.1268884487885);

// after 3 bounces
// Solution 31
// 1.0e-11
// 32 solution(s)
// split:  33
// hnr:    67
// narrow: 150
// time: 625
//
// 1.0e-10
// 4 solution(s)
// split:  5
// hnr:    11
// narrow: 38
// time: 281
// time = [2.6883363073955313, 2.6883363074291093]
// x  = [12.627623749238515, 12.627623749238783]
// y  = [0.061214838983087883, 0.06121483898335351]
// vx = [4.2385382500314046, 4.2385382500314064]
// vy = [-6.0528175787230989, -6.0528175787147207]
// nvx = [-5.0165116813535491, -5.0165116811120214]
// nvy = [3.2196217626885062, 3.2196217627150858]
*/
